
#ifndef ROBOT_JOINTSPACESOLVER
#define ROBOT_JOINTSPACESOLVER


/** Joint Space Solver
 * Uses Inverse Kinematics, Jacobian and Dynamics Solvers to map from cartesian
 * end effector position, velocity and acceleration to joint positions, 
 * velocities and accelerations
 * 
 * Checks velocities and accelerations using Forward Kinematics?
 * 
 */
 
 // This class is put forward to replace inversekinematics.hpp and jacobian.hpp

#include "robotStruct.hpp"
#include "termcolors.hpp"

// Return Codes for solver:
#define SOLVER_SUCCESS 0
#define IK_WARN_NON_DEXTROUS 1
#define IK_ERROR_UNREACHABLE 2
#define FK_ERROR_FAILED 3
#define JACOBIAN_ERROR_SINGULARITY 4

// Specify behaviour when the endangle is fit to a solution:
// Should the solver edit the endangle in place or change a copy
#define IK_ENDANGLE_MODIFY_PARAM 5
#define IK_ENDANGLE_MODIFY_COPY 6

class JointSpaceSolver {
 public:
  JointSpaceSolver(RobotModel* rm);
  ~JointSpaceSolver();
  
  int solve(Setpoint* result);
  double getReachableLimit();
  int solveIK(cvec4d* cpos, jvec4d* jpos, int editMode = IK_ENDANGLE_MODIFY_PARAM);
  int solveFK(jvec4d* jpos, cvec4d* cpos);
  
 private:
  RobotModel* rm;
  
};


inline JointSpaceSolver::JointSpaceSolver(RobotModel* a_rm) {
  rm = a_rm;
}

inline JointSpaceSolver::~JointSpaceSolver() {
  // Destructor
}

inline int JointSpaceSolver::solve(Setpoint* sp) {
  int errcode = 0;
  
  int ik_errcode = solveIK(sp->cpos, sp->jpos);
  if(ik_errcode > 1) return ik_errcode;
  
  return errcode;
}


/** solveIK:
 * 
 * Solve inverse kinematics from end effector coords: endx, endy, endz
 * and end effector angle: endangle
 * populates jpos vector (must be size 4)
 * 
 * returns int errorcode:
 *  0: success
 *  1: limited endangle - reachable but not dextrous location: endangle ignored
 *  2: failure - unreachable location: endx, endy, endz
 */
inline int JointSpaceSolver::solveIK(cvec4d* end, jvec4d* jpos, 
                                     int editMode) {
  // Convert to birds-eye angle and radius:
  jpos->t1 = atan2(end->y, end->x);
  double endr = sqrt(end->y*end->y + end->x*end->x);
  double reach = sqrt(endr*endr + ( end->z-rm->l1 )*( end->z-rm->l1 ));
  double maxDist = getReachableLimit();
  
  // If all the links are outstretched can we reach it:
  // Note ignore l2 so reaching up is satisfied?
  if(reach > maxDist) {
    std::cout << FONT_ERROR << "Needs to reach " << reach << "radially\n";
    std::cout << "Reachable area maximum radius is " << maxDist << "\n";
    std::cout << "IK: unreachable location\n" << FONT_RESET;
    return IK_ERROR_UNREACHABLE;  // unreachable
  }
  
  // Find fourth joint location in radial space: (r,z)
  double r4 = endr - rm->l5*cos(end->phi);
  double z4 = end->z + rm->l5*sin(end->phi);

  // Find solution to elbow:
  double dx = r4 - rm->l2;
  double dy = z4 - rm->l1;
  double dlen = sqrt(dx*dx + dy*dy);
  double inAng1, inAng2, inAng3;
  // Check whether a solution exists for this endangle: 
  if(dlen < rm->l3+rm->l4) {
    // Solution exists:
    // Find inner jpos to the triangle formed by l3 and l4 and dlen:
    // inAng1 is angle between dlen and l3:
    inAng1 = acos( (dlen*dlen + rm->l3*rm->l3 - rm->l4*rm->l4)/(2*dlen*rm->l3) );
    // inAng2 is angle between l3 and l4:
    inAng2 = acos( (rm->l3*rm->l3 + rm->l4*rm->l4 - dlen*dlen)/(2*rm->l3*rm->l4) );
    // inAng3 is the angle between horizontal and dlen:
    inAng3 = atan2(dy, dx);
  
    // always return solution #1 of 2 for now:
    jpos->t2 = inAng3 + inAng1;
    //jpos->t2 = inAng3 - inAng1;
    jpos->t3 = inAng2 - M_PI;
    
    jpos->t4 = -(end->phi + jpos->t2 + jpos->t3);
    
    // Ensure we dont obstruct the base:
    if(jpos->t2 > M_PI || jpos->t2 < -M_PI/2) {
      std::cout << FONT_ERROR << "IK: Solution obstructs base!\njpos: ("
                << jpos->t1 << "," << jpos->t2 << "," << jpos->t3
                << "," << jpos->t4
                << ")\nto reach pos: (" << end->x << "," << end->y << ","
                << end->z << ")\n" << FONT_RESET;
      return IK_ERROR_UNREACHABLE;
    }
    
    return SOLVER_SUCCESS;
  } else {
    // Find rejected endangle solution for reachable but not dextrous space:
    std::cout << FONT_WARN << "IK: Non-dextrous solution\n";
    
    dx = endr - rm->l2;
    dy = end->z - rm->l1;
    dlen = sqrt(dx*dx + dy*dy);
    // Find inner jpos to the triangle formed by l3+l4 and l5 and dlen:
    // inAng1 is angle between dlen and l3:
    inAng1 = acos( ((rm->l3+rm->l4)*(rm->l3+rm->l4) + dlen*dlen - rm->l5*rm->l5)/(2*dlen*(rm->l3+rm->l4)) );
    // inAng3 is the angle between horizontal and dlen:
    inAng3 = atan2(dy, dx);
    
    // Redefine endangle:
    double endangle = asin( (rm->l3+rm->l4)*sin(inAng1)/rm->l5 );
    std::cout << "Forced end angle to " << endangle*180/M_PI << "\n" << FONT_RESET;
    
    if(editMode == IK_ENDANGLE_MODIFY_PARAM) end->phi = endangle;
    
    // always return solution #1 of 2 for now:
    jpos->t2 = inAng3 + inAng1;
    //jpos->t2 = inAng3 - inAng1;
    jpos->t3 = 0.0;
    
    jpos->t4 = -(endangle + jpos->t2);
    
    if(abs(jpos->t4) > M_PI) {
      std::cout << FONT_ERROR << "Unreachable location\n" << FONT_RESET;
      return IK_ERROR_UNREACHABLE;
    }
    
    return IK_WARN_NON_DEXTROUS;
  }
}

inline double JointSpaceSolver::getReachableLimit() {
  return rm->l2+rm->l3+rm->l4+rm->l5;
}

inline int JointSpaceSolver::solveFK(jvec4d* jpos, cvec4d* cpos) {
	cpos->x = cos(jpos->t1)*(rm->l2 + rm->l4*cos(jpos->t2 + jpos->t3) + rm->l3*cos(jpos->t2) + rm->l5*cos(jpos->t2 + jpos->t3 + jpos->t4));
	cpos->y = sin(jpos->t1)*(rm->l2 + rm->l4*cos(jpos->t2 + jpos->t3) + rm->l3*cos(jpos->t2) + rm->l5*cos(jpos->t2 + jpos->t3 + jpos->t4));
	cpos->z =  rm->l1 + rm->l4*sin(jpos->t2 + jpos->t3) + rm->l3*sin(jpos->t2) + rm->l5*sin(jpos->t2 + jpos->t3 + jpos->t4);
	cpos->phi = - jpos->t2 - jpos->t3 - jpos->t4;
  
  return SOLVER_SUCCESS;
}


#endif
